#ifndef ODOM_CALIB_HPP_
#define ODOM_CALIB_HPP_

#include <rclcpp/rclcpp.hpp>
#include <memory>
#include <thread>
#include <geometry_msgs/msg/twist.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <agv_msgs/msg/agv_odom_calib.hpp>
#include <agv_msgs/srv/odom_calib.hpp>
#include <agv_msgs/msg/attitude.hpp>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <cmath>

class OdomCalib : public rclcpp::Node
{
public:
  OdomCalib();
  ~OdomCalib() {}

  bool initparams();
  void OdomCalibStart();
  void shutdown() { shutdown_req = true; shutdown_vel = true;};

  double getdistance(geometry_msgs::msg::Pose cur, geometry_msgs::msg::Pose des);
  float EstimateArea(float fStartValue, float fEndValue, float fSlope);
  float Approach(float from, float to, float step);

  double getRadDistance(double target, double current);
  double getRemainder(double x, double y);
  double normalizationRad(double rad);

private:
  void OdomSubCallback(const agv_msgs::msg::OdometryLite::SharedPtr msg);
  void OdomCalibCallback(
    const std::shared_ptr<agv_msgs::srv::OdomCalib::Request> request,
    std::shared_ptr<agv_msgs::srv::OdomCalib::Response> response);
  void KeyvelSubCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  bool Distance();
  bool Anglar();

  int start;
  int mode;
  double target_vel;
  double oprange;

  bool shutdown_req;
  bool shutdown_vel;
  std::thread calib_thread;
  
  geometry_msgs::msg::Twist current_vel;
  geometry_msgs::msg::Pose current_pose;
  geometry_msgs::msg::Pose start_pose;

  rclcpp::Subscription<agv_msgs::msg::OdometryLite>::SharedPtr odometry_sub;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr keyvel_sub;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;
  rclcpp::Publisher<agv_msgs::msg::AgvOdomCalib>::SharedPtr calib_pub;
  rclcpp::Service<agv_msgs::srv::OdomCalib>::SharedPtr odom_server;
};

class ImuCalib : public rclcpp::Node
{
public:
    ImuCalib();
    ~ImuCalib() {};

private:
    void imu_attitude_callback(const agv_msgs::msg::Attitude::SharedPtr msg);

    rclcpp::Subscription<agv_msgs::msg::Attitude>::SharedPtr imu_attitude_sub_;

    double start_yaw_;
    double end_yaw_;
};

#endif /* ODOM_CALIB_HPP_ */